#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import serial#type: ignore
import math
import time
import numpy as np
from typing import Union

class SerialControl:
    def __init__(self, port: str, baudrate: int, timeout: int = 1):
        """
        初始化串口配置
        :param port: 串口号，例如："/dev/ttyUSB0"
        :param baudrate: 波特率，例如：9600
        :param timeout: 超时时间（秒），默认为1秒
        """
        # 初始化串口参数
        self.id_map = {str(i): f"{i:03d}" for i in range(0, 30)}
        self.ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            parity=serial.PARITY_NONE,  # 无校验位
            stopbits=serial.STOPBITS_ONE,  # 1位停止位
            bytesize=serial.EIGHTBITS,  # 8位数据位
            timeout=timeout  # 超时时间
        )

        # 生成舵机ID映射表（0-29转为三位字符表示，如0 → 000，15 → 015，依此类推）

        # 尝试打开串口（自动重试机制）
        while True:
            try:
                if self.ser.is_open:
                    print("串口已打开，正在自检...")
                    time.sleep(1.5)  # 等待自检时间
                    print("自检已完成，准备接受指令...")
                    break
            except serial.SerialException as e:
                print(f"串口打开失败，错误信息：{e}, 请检查串口号和权限。")
                time.sleep(1.5)  # 重试间隔
    def servoRads(self, servoRads: list[list[Union[int,float]]],surpress_print:bool=False,use_seq:bool=True):
        # if use_seq:
        #     seqMap={
        #         1:[1,2,3,18,17,16],
        #         2:[4,5,6,10,11,12],
        #         3:[7,8,9,13,14,15]
        # }
        #     for seq in seqMap:
        #         curServoRads=[]
        #         for servo in servoRads:
        #             if servo[0] in seqMap[seq]:
        #                 curServoRads.append(servo)
        #         if len(curServoRads)>0:
        #             print(f"curServoRads: {curServoRads}")
        #             command = self._servoRadsTostr(curServoRads)
        #             self.sendStr(command,surpress_print)
        #             time.sleep(1)
        # else:
        # for servo in servoRads:
        command = self._servoRadsTostr(servoRads)
        self.sendStr(command,surpress_print)
        time.sleep(0.5)
    def _servoRadsTostr(self,servoRadsOrRads: list[list[Union[int,float]]]) -> str:
        """
        生成写入关节控制命令
        :param rads: 关节角度列表（弧度制）
        :return: 控制命令字符串
        """
        print("Control arm...")
        command_parts = []
        servoRads=[]
        if not(isinstance(servoRadsOrRads[0],list) or isinstance(servoRadsOrRads[0],tuple)):
            for motori,rad in enumerate(servoRadsOrRads):
                servoRads.append([motori+21,rad])
        else:
            servoRads=servoRadsOrRads
        for motori,rad in servoRads:
            if rad=="SCK":
                command_parts.append(f"#{(motori):03d}PSCK!")
            else:
                if motori != 24:
                    pwm = rad * 4000 / (3 * math.pi) + 1500  
                else:
                    pwm = 1500 - rad * 4000 / (3 * math.pi)
                
                command_parts.append(f"#{(motori):03d}P{round(pwm):04d}T1500!")
        return "{" + "".join(command_parts) + "}"
    def _generate_read_command(self, machine: str = "arm") -> list:
        """
        （私有）生成读取关节状态的命令
        :param machine: 控制设备类型，默认为 "arm"
        :return: 读取命令列表
        """
        command_list = []
        if machine == "arm":
            print("read from arm")
            for i in range(5):
                # 生成读取舵机状态的命令（ID 21-25）
                command_list.append(f"#{self.id_map[str(i + 21)]}PRAD!")
        elif machine == "legs":
            print("read from legs")
            for i in range(1,19):
                command_list.append(f"#{self.id_map[str(i)]}PRAD!")
        else:
            print("Unknown robot part!")
        return command_list
    def sendStr(self, command,surpress_print:bool=False):
        if not surpress_print:
            print(command)
        self.ser.write(command.encode())
        PWMFeedback=self.ser.read_all()
        if not surpress_print:
            print(PWMFeedback.decode('ascii', errors='ignore'))

    def get_rads_data(self, machine: str = "arm") -> list:
        """
        获取关节角度反馈数据（弧度制）
        :param machine: 控制设备类型，默认为 "arm"
        :return: 关节角度列表（弧度制）
        """
        # 生成读取命令
        read_commands = self._generate_read_command(machine=machine)

        PWMFeedbackList = []
        # 逐条发送读取命令，获取反馈数据
        for cmd in read_commands:
            self.ser.write(cmd.encode())
            time.sleep(0.2)  # 等待舵机响应
            PWMFeedback = self.ser.read_all()
            PWMFeedbackList.append(PWMFeedback.decode('ascii', errors='ignore'))
        print(f"PWMFeedback: {PWMFeedbackList}")
        try:
            return self.parse_PWMFeedbackList(PWMFeedbackList,machine)
        except Exception as e:
            print(f"解析反馈数据时出错：{e}")
            return []


    @staticmethod
    def parse_PWMFeedbackList(PWMFeedbackList: list,machine: str = "arm") -> list:
        """
        解析原始串口反馈数据，转换为关节角度（弧度制）
        :param PWMFeedbackList: 原始反馈数据列表
        :return: 关节角度列表（弧度制）
        """
        PWMList = []
        for item in PWMFeedbackList:
            # print(f"item: {item}")
            # 检查数据长度，不足的不处理
            if len(item) < 5:
                continue
            # 提取最后四位作为PWM值
            pwm_str = item[-5:-1]
            if pwm_str.isdigit():
                pwm = int(pwm_str)
                # 检查PWM值是否在有效范围内
                if 500 <= pwm <= 2500:
                    PWMList.append(pwm)

        # 转换PWM值为关节角度
        radList = []
        if machine == "arm":
            for idx, pwm in enumerate(PWMList[:5]):  # 只处理前5个关节
                if idx != 3:
                    # 标准转换：(中位值1500 - 实际PWM) * 转换系数
                    rad = (-1500 + pwm) * 3 * math.pi / 4000
                else:
                    # 第4关节（索引3）方向相反
                    rad = (-pwm + 1500) * 3 * math.pi / 4000
                radList.append(rad)
        elif machine == "legs":
            for idx, pwm in enumerate(PWMList):
                rad = (-1500 + pwm) * 3 * math.pi / 4000
                radList.append(rad)
        degList=[]  
        for rad in radList:
            degList.append(rad*180/math.pi)
        return degList


    def close_serial(self):
        """
        关闭串口连接
        """
        self.ser.close()
# if __name__ == "__main__":

